package org.example.util;

import org.example.MapDrawing;
import org.example.bean.Cord;
import org.example.bean.GameMap;
import org.example.bean.LazyNode;
import org.example.bean.Node;

import javax.swing.*;
import java.util.*;

/**
 * @Desc:
 * @Author: yanghongjun
 * @Date: 2024/5/14
 **/
public class ThetaStarUtil {


    private static final double DIAG_COST = 1.41421356237; // sqrt(2), 代表对角线移动的成本

    private static final int[][] moves = {{-1, 0}, {1, 0}, {0, -1}, {0, 1}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1}};

    public static void main(String[] args) {
        long now = System.currentTimeMillis();
        GameMap gameMap = new GameMap(0, 1200, 0, 1200);
        int[][] maps = new int[1200][1200];
        AStarSearch.initMapGridBlockArray(maps);
        gameMap.setMaps(maps);
        List<LazyNode> path1 = ThetaStarUtil.findPath(new LazyNode(60, 815), new LazyNode(52, 679), gameMap);
        System.out.println("ThetaStarUtil 耗时：" + (System.currentTimeMillis() - now));
        //画图
        if (path1 != null) {
            List<Cord> list = new ArrayList<>(path1.size());
            for (LazyNode node : path1) {
                list.add(new Cord((int) node.getX(), (int) node.getY()));
            }
            SwingUtilities.invokeLater(() -> {
                MapDrawing gameDraw = new MapDrawing(maps, list, "ThetaStarUtil寻路");
                gameDraw.setVisible(true);
            });
        }
    }


    /**
     * 使用Theta*算法找到从起点到终点的路径。
     *
     * @param startNode 起点
     * @param endNode  终点
     * @param gameMap   地图网格，0表示可通行，非0表示障碍物
     * @return 从起点到终点的路径，若无法到达则返回null
     */
    public static List<LazyNode> findPath(LazyNode startNode, LazyNode endNode, GameMap gameMap) {
        long now = System.currentTimeMillis();
        PriorityQueue<LazyNode> openSet = new PriorityQueue<>(Comparator.comparingDouble(LazyNode::getfScore));
        HashSet<LazyNode> closedSet = new HashSet<>();

//        startNode.gScore = 0;
//        startNode.hScore = heuristic(startNode, goalNode);
//        startNode.fScore = startNode.gScore + startNode.hScore;

        startNode.setgScore(0);
        startNode.sethScore(heuristic(startNode, endNode));
        startNode.setfScore(startNode.getgScore() + startNode.gethScore());
        openSet.add(startNode);

        while (!openSet.isEmpty()) {
            LazyNode currentNode = openSet.poll();
            currentNode.setInOpenSet(false);

            if (currentNode.equals(endNode)) {
                List<LazyNode> lazyNodes = reconstructPath(currentNode);
                System.out.println("ThetaStarUtil 寻路耗时 " + (System.currentTimeMillis() - now));
                long l = System.currentTimeMillis();
                List<LazyNode> lazyNodes1 = optimizePath(lazyNodes, gameMap.getMaps());
                System.out.println("openSet size：" + openSet.size() + "-----" + 1200 * 1200);
                System.out.println("ThetaStarUtil 平滑路径耗时：" + (System.currentTimeMillis() - l) + " 平滑前节点：" + lazyNodes.size() +
                        "平滑后节点：" + lazyNodes1.size());
                return lazyNodes1;
            }

            closedSet.add(currentNode);

            for (LazyNode neighbor : gameMap.getNeighbors(currentNode)) {
                if (closedSet.contains(neighbor)) {
                    continue;
                }

                double tentativeGScore = currentNode.getgScore() + distBetween(currentNode, neighbor);

                if (!neighbor.isInOpenSet() || tentativeGScore < neighbor.getgScore()) {
                    neighbor.setParent(currentNode);
                    neighbor.setgScore(tentativeGScore);
                    neighbor.sethScore(heuristic(neighbor, endNode));
                    neighbor.setfScore(neighbor.getgScore() + neighbor.gethScore());
                    openSet.add(neighbor);
                    neighbor.setInOpenSet(true);
                }
            }
        }

        return null;
    }

    /**
     * 根据给定的当前节点，重建并返回从起点到该节点的路径。
     *
     * @param currentNode 当前节点
     * @return 从起点到当前节点的路径
     */
    private static List<LazyNode> reconstructPath(LazyNode currentNode) {
        List<LazyNode> path = new ArrayList<>();
        int depth = 0;
        while (currentNode != null) {
            if (depth == 0 || depth != currentNode.getDirection() || currentNode.getParent() == null) {
                path.add(currentNode);
                depth = currentNode.getDirection();
            }
            currentNode = currentNode.getParent();
        }
        Collections.reverse(path);
        return path;
    }

    /**
     * 获取给定节点的所有邻居节点。
     *
     * @param node 给定节点
     * @param grid 地图网格
     * @return 邻居节点列表
     */
    private static List<LazyNode> getNeighbors(LazyNode node, int[][] grid) {
        List<LazyNode> neighbors = new ArrayList<>();
        for (int[] move : moves) {
            int nextX = node.getX() + move[0];
            int nextY = node.getY() + move[1];

            if (nextX >= 0 && nextY >= 0 && nextX < grid.length && nextY < grid[0].length && grid[nextX][nextY] == 0) {
                neighbors.add(new LazyNode(nextX, nextY));
            }
        }

        return neighbors;
    }

    /**
     * 计算两个节点之间的启发式距离。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 启发式距离
     */
    private static double heuristic(LazyNode nodeA, LazyNode nodeB) {
        return Math.abs(nodeA.getX() - nodeB.getX()) + Math.abs(nodeA.getY() - nodeB.getY()) * 10;
    }

    /**
     * 计算两个节点之间的距离。
     *
     * @param nodeA 节点A
     * @param nodeB 节点B
     * @return 两点之间的距离
     */
    private static double distBetween(LazyNode nodeA, LazyNode nodeB) {
        double dx = nodeA.getX() - nodeB.getX();
        double dy = nodeA.getY() - nodeB.getY();

        if (dx == 0 || dy == 0) {
            return 1; // Cardinal move
        } else {
            return DIAG_COST; // Diagonal move
        }
    }

    /**
     * 优化路径，通过去除不必要的节点来简化路径。
     *
     * @param path 原始路径
     * @param grid 地图网格
     * @return 优化后的路径
     */
    public static List<LazyNode> optimizePath(List<LazyNode> path, int[][] grid) {
        if (path == null || path.size() < 3) {
            return path; // 路径不足3个点时无需优化
        }

        List<LazyNode> optimizedPath = new ArrayList<>();
        optimizedPath.add(path.get(0)); // 起点总是加入优化路径

        LazyNode lastOptimizedNode = path.get(0);
        for (int i = 1; i < path.size() - 1; i++) {
            LazyNode currentNode = path.get(i);
            LazyNode nextNode = path.get(i + 1);

            // 检查当前节点是否可以与下一个节点直线相连而不穿过障碍物
            if (canConnectStraight(lastOptimizedNode, nextNode, grid)) {
                // 如果可以直线相连，则跳过当前节点
                continue;
            }

            // 否则，将当前节点添加到优化路径中
            optimizedPath.add(currentNode);
            lastOptimizedNode = currentNode;
        }

        optimizedPath.add(path.get(path.size() - 1)); // 终点总是加入优化路径
        return optimizedPath;
    }

    /**
     * 检查两个节点是否可以直线相连而不穿过障碍物。
     *
     * @param startNode 起点节点
     * @param endNode   终点节点
     * @param grid      地图网格
     * @return 如果两点之间可以直线相连并且没有障碍物则返回true，否则返回false
     */
    private static boolean canConnectStraight(LazyNode startNode, LazyNode endNode, int[][] grid) {
        int x1 = startNode.getX();
        int y1 = startNode.getY();
        int x2 = endNode.getX();
        int y2 = endNode.getY();
        // 计算两点之间的差值
        int dx = Math.abs(x2 - x1);
        int dy = Math.abs(y2 - y1);
        // 确定x和y的增量方向
        int sx = (x1 < x2) ? 1 : -1;
        int sy = (y1 < y2) ? 1 : -1;
        // 用于决定是沿x轴还是y轴移动
        int err = dx - dy;
        // 使用Bresenham的线算法遍历两点之间的所有点
        while (true) {
            // 检查当前点是否在网格内且不是障碍物
            if (isInBounds(x1, y1, grid) && grid[x1][y1] != 0) {
                return false; // 路径上有障碍物
            }
            // 如果已经到达终点，则跳出循环
            if (x1 == x2 && y1 == y2) {
                break; // 到达终点
            }
            // 更新误差项
            int e2 = err * 2;
            // 根据误差项决定是沿x轴还是y轴移动
            if (e2 > -dy) {
                err -= dy;
                x1 += sx;
            }
            if (e2 < dx) {
                err += dx;
                y1 += sy;
            }
        }
        // 如果能够顺利到达终点，则说明两点之间可以直线相连
        return true; // 路径上无障碍物
    }

    /**
     * 检查给定的坐标是否在网格的边界内。
     *
     * @param x    坐标x
     * @param y    坐标y
     * @param grid 地图网格
     * @return 如果坐标在网格内则返回true，否则返回false
     */
    private static boolean isInBounds(int x, int y, int[][] grid) {
        return x >= 0 && x < grid.length && y >= 0 && y < grid[0].length;
    }
}
